#ifndef SCANNERQR_HPP
#define SCANNERQR_HPP

#include "codeqr.hpp"
#include <zbar.h>
#include <iostream>
#include <fstream>

#include <atomic>
#include <thread>
#include <chrono>


class CameraParam
{
public:
    cv::Mat K;
    cv::Mat D;
    cv::Vec6d RT;

    CameraParam()
    {

    }

    void operator =(const CameraParam &src)
    {
        src.K.copyTo(K);
        src.D.copyTo(D);
        RT=src.RT;
    }

    CameraParam(const CameraParam &src)
    {
        operator =(src);
    }

};


class CodeQRA
        : public CodeQR
{
public:
    int viewIndex;
    cv::Vec6d position;
public:
    CodeQRA()
        : CodeQR()
    {
        
    }
    
    void CenterPos(CameraParam &cp)
    {
        cv::Vec6d rt=Center(cp.K,cp.D);        
        cv::Vec3d rv3, tv3;
        cv::composeRT(cv::Vec3d(cp.RT[0], cp.RT[1], cp.RT[2]), cv::Vec3d(cp.RT[3], cp.RT[4], cp.RT[5]),
                cv::Vec3d(rt[0], rt[1], rt[2]), cv::Vec3d(rt[3], rt[4], rt[5]), rv3, tv3);
        position=cv::Vec6d(rv3[0],rv3[1],rv3[2],tv3[0],tv3[1],tv3[2]);
    }
    
};


class CameraQR
{


public:
    static void EnqueueCodeQR(std::vector<CodeQRA> &codes, CodeQRA &newCode, float xThreshold, float yThreshold)
    {
        cv::Point2f newCenter=newCode.Center();
        for(size_t i=0;i<codes.size();i++)
        {
            cv::Point2f center=codes[i].Center();
            if(fabs(center.x-newCenter.x)<xThreshold && fabs(center.y-newCenter.y)<yThreshold)
                return;
        }
        codes.push_back(newCode);
    }


    static void EnqueueCodeQR3D(std::vector<CodeQRA> &codes, CodeQRA &newCode, cv::Vec3d xyzThreshold)
    {
        cv::Vec6d newCenter=newCode.position;
        for(size_t i=0;i<codes.size();i++)
        {
            cv::Vec6d center=codes[i].position;
            if(fabs(center[3]-newCenter[3])<xyzThreshold[0]
                    && fabs(center[4]-newCenter[4])<xyzThreshold[1]
                    && fabs(center[5]-newCenter[5])<xyzThreshold[2] )
                return;
        }
        codes.push_back(newCode);
    }


    static void UpScaleScanROI(cv::Mat *grayImage, zbar::ImageScanner &scanner, std::vector<CodeQRA> &codes, std::atomic_flag *lockCodes,
                               cv::Point &origin, cv::Size &roiSize, int interpolate=cv::INTER_LINEAR, double scale=6)
    {
        cv::Mat mgs;
        cv::resize((*grayImage)(cv::Rect(origin, roiSize)),mgs,cv::Size(),scale, scale, interpolate);

        zbar::Image imageOne(mgs.cols, mgs.rows, "Y800", (uchar *)mgs.data, mgs.cols*mgs.rows);

        scanner.set_config(zbar::ZBAR_NONE, zbar::ZBAR_CFG_ENABLE, 0);
        scanner.set_config(zbar::ZBAR_NONE, zbar::ZBAR_CFG_X_DENSITY, 1);
        scanner.set_config(zbar::ZBAR_NONE, zbar::ZBAR_CFG_Y_DENSITY, 1);
        scanner.set_config(zbar::ZBAR_QRCODE, zbar::ZBAR_CFG_ENABLE, 1);

        scanner.scan(imageOne);

        // extract results
        for(zbar::Image::SymbolIterator symbol = imageOne.symbol_begin();
            symbol != imageOne.symbol_end();
            ++symbol)
        {
            if(symbol->get_type_name().compare("QR-Code")==0)
            {
                CodeQRA code1;
                code1.data=symbol->get_data();
                int n = symbol->get_location_size();
                for(int i=0;i<n;i++)
                {
                    code1.vertex.push_back(cv::Point(symbol->get_location_x(i)/scale,symbol->get_location_y(i)/scale)+origin);
                }

                while (lockCodes->test_and_set()) {
                    std::cout<<"wait lockCodes\n"<<std::flush;
                }
                EnqueueCodeQR(codes, code1, 20, 20);
                lockCodes->clear();
            }
        }
    }

    static void CalGrid(std::vector<cv::Point> &topLeft, cv::Size &areaSize, cv::Size imageSize, cv::Size overlapSize, cv::Size gridSize)
    {
        areaSize.width=(imageSize.width+(gridSize.width-1)*overlapSize.width)/gridSize.width;
        areaSize.height=(imageSize.height+(gridSize.height-1)*overlapSize.height)/gridSize.height;

        cv::Point tl(0,0);
        topLeft.clear();

        for(int i=0;i<gridSize.width;i++)
        {
            tl.y=0;
            for(int j=0;j<gridSize.height;j++)
            {
                topLeft.push_back(tl);

                tl.y+=areaSize.height-overlapSize.height;
            }
            tl.x+=areaSize.width-overlapSize.width;
        }
    }


    static void UpScaleScan(cv::Mat &grayImage, std::vector<CodeQRA> &codes,
                            int interpolate=cv::INTER_LINEAR, double scale=6)
    {
        clock_t t=clock();
        std::atomic_flag lockOrder=ATOMIC_FLAG_INIT;

        cv::Size ovsz(grayImage.cols/20, grayImage.rows/15);
        std::vector<cv::Point> tls;
        cv::Size asz;

        const cv::Size segment(6,5);

        zbar::ImageScanner scanner[segment.area()];
        CalGrid(tls, asz, grayImage.size(), ovsz, segment);

        std::vector<std::thread> ths;

        for(int i=0;i<tls.size();i++)
        {
            ths.push_back(std::thread(UpScaleScanROI, &grayImage, std::ref(scanner[i]), std::ref(codes), &lockOrder, std::ref(tls[i]), std::ref(asz), interpolate, scale));
        }

        for(int i=0;i<ths.size();i++)
            ths[i].join();

        t = clock() - t;

        printf ("UpScaleScan time %d clicks (%f seconds).\n",t,((float)t)/CLOCKS_PER_SEC);

    }

public:
    CameraParam cameraParam;
    std::vector<CodeQRA> codes;
public:
    CameraQR()
    {

    }

    void Scan(cv::Mat &frame, int index=0, int n=10)
    {



        cv::VideoCapture c(index);

        c.set(cv::CAP_PROP_FRAME_HEIGHT, 2448);
        c.set(cv::CAP_PROP_FRAME_WIDTH, 3264);
        c.set(cv::CAP_PROP_SATURATION, 0);
        c.set(cv::CAP_PROP_AUTO_EXPOSURE, 0);
        c.set(cv::CAP_PROP_EXPOSURE, -3);
        c.set(cv::CAP_PROP_SHARPNESS, 0);
        c.set(cv::CAP_PROP_CONTRAST, 64);
//        c.set(cv::CAP_PROP_BRIGHTNESS, 64);


        int exp[]={-4, -3, -2, -1};

        cv::Mat mg;

        for(int i=0;i<8;i++)
        {
            std::cout<<i<<'\n'<<std::flush;
            c.read(frame);
        }

        clock_t t=clock();

        for(int j=0;j<4;j++)
        {
            c.set(cv::CAP_PROP_EXPOSURE, exp[j]);

            for(int i=0;i<n;i++)
            {
                std::cout<<i<<'\n'<<std::flush;
                c.read(frame);
                cv::cvtColor(frame, mg, CV_BGR2GRAY);
                UpScaleScan(mg, codes, cv::INTER_NEAREST, 3);
                std::cout<<codes.size()<<" codes found\n"<<std::flush;
                UpScaleScan(mg, codes, cv::INTER_LINEAR, 3);
                std::cout<<codes.size()<<" codes found\n"<<std::flush;
//                UpScaleScan(mg, codes, cv::INTER_CUBIC, 3);
//                std::cout<<codes.size()<<" codes found\n"<<std::flush;
//                UpScaleScan(mg, codes, cv::INTER_LANCZOS4, 3);
//                std::cout<<codes.size()<<" codes found\n"<<std::flush;
            }
        }



        for(size_t i=0;i<codes.size();i++)
        {
            codes[i].CenterPos(cameraParam);
            codes[i].viewIndex=index;
        }

        t = clock() - t;

        printf ("Scan time %d clicks (%f seconds).\n",t,((float)t)/CLOCKS_PER_SEC);

    }

    void Draw(cv::Mat &canvas, cv::Scalar color=cv::Scalar(255,0,0))
    {
        for(size_t i=0;i<codes.size();i++)
        {
            cv::polylines(canvas, codes[i].vertex, true, color, 1);
        }
    }


};


class ScannerQR
{
public:


    static cv::Mat ReadSmooth(int index=0, int n=100)
    {
        cv::VideoCapture c(index);

        cv::Mat m;
        c.set(cv::CAP_PROP_FRAME_HEIGHT, 2448);
        c.set(cv::CAP_PROP_FRAME_WIDTH, 3264);
        c.set(cv::CAP_PROP_SATURATION, 0);
        //    c.set(cv::CAP_PROP_EXPOSURE, -3);
        c.set(cv::CAP_PROP_SHARPNESS, 0);
        c.set(cv::CAP_PROP_CONTRAST, 64);
        c.set(cv::CAP_PROP_BRIGHTNESS, 64);


        cv::namedWindow("ms",CV_WINDOW_NORMAL);
        cv::waitKey(1000);
        cv::Mat ms;

        for(int i=0;i<10;i++)
        {
            std::cout<<i<<'\n'<<std::flush;
            c.read(m);
        }

        for(int i=0;i<n;i++)
        {

            std::cout<<i<<'\n'<<std::flush;
            c.read(m);
            cv::Mat mg;
            cv::cvtColor(m, mg, CV_BGR2GRAY);
            cv::Mat mg32;
            mg.convertTo(mg32,CV_32SC1);

            if(ms.empty())
                mg32.copyTo(ms);
            else
                ms=ms+mg32;

        }

        ms=ms*(1/(double)n);

        cv::Mat ms8;
        ms.convertTo(ms8,CV_8UC1);

        cv::imshow("ms",ms8);

        cv::imwrite("ms8.png",ms8);

        cv::waitKey(1000);

        c.release();

        return ms8;
    }





    static void test(std::string fp)
    {
        cv::Mat mg=cv::imread(fp,0);//(cv::Rect(cv::Point(1817,229),cv::Point(3171,1021)));

        //        cv::Mat mg=ReadSmooth(0,10);//(cv::Rect(cv::Point(1639,422),cv::Point(3161,1814)));

        std::vector<CodeQRA> codes;

        CameraQR::UpScaleScan(mg, codes, cv::INTER_NEAREST, 4);
        CameraQR::UpScaleScan(mg, codes, cv::INTER_LINEAR, 4);
        CameraQR::UpScaleScan(mg, codes, cv::INTER_CUBIC, 4);
        CameraQR::UpScaleScan(mg, codes, cv::INTER_LANCZOS4, 4);

        std::cout<<codes.size()<<" codes found\n"<<std::flush;

        cv::Mat m;
        cv::cvtColor(mg,m,CV_GRAY2BGR);

        for(size_t i=0;i<codes.size();i++)
        {
            cv::polylines(m, codes[i].vertex, true, cv::Scalar(255,0,0), 1);
        }


        cv::namedWindow("leftCam",CV_WINDOW_NORMAL);
        cv::imshow("leftCam", m);
        cv::imwrite("res-5.png",m);
        cv::waitKey(1000);
    }


    static void test2(int index=1, int n=1)
    {

        CameraQR cqr;

        cv::Mat m;
        cqr.Scan(m, index, n);

        cqr.Draw(m);

        cv::namedWindow("leftCam",CV_WINDOW_NORMAL);
        cv::imshow("leftCam", m);
        cv::imwrite("res-5.png",m);
        cv::waitKey(1000);
    }


    
    static void test3()
    {
        CameraQR cqr[2];
        cv::Mat m[2];
        
        double scaleI=3264.0/640.0;
        
        cqr[0].cameraParam.K=(cv::Mat_<double>(3,3)<<5.2790986585867722e+002*scaleI, 0., 3.1681421549256817e+002*scaleI, 0.,
                5.2790986585867722e+002*scaleI, 2.3068252335840023e+002*scaleI, 0., 0., 1.);
        cqr[0].cameraParam.D=(cv::Mat_<double>(5,1)<<-1.4342224804427140e-002, -2.4705616674454523e-002,
                              -3.6992103942241744e-005, -6.0099840001129334e-004, 0.);
        cqr[0].cameraParam.RT=cv::Vec6d(0,0,0,0,0,0);
        
        
        cqr[1].cameraParam.K=(cv::Mat_<double>(3,3)<<5.2790986585867722e+002*scaleI, 0., 3.1681421549256817e+002*scaleI, 0.,
                5.2790986585867722e+002*scaleI, 2.3068252335840023e+002*scaleI, 0., 0., 1.);
        cqr[1].cameraParam.D=(cv::Mat_<double>(5,1)<<-1.4342224804427140e-002, -2.4705616674454523e-002,
                              -3.6992103942241744e-005, -6.0099840001129334e-004, 0.);
        cqr[1].cameraParam.RT=cv::Vec6d(0,0,0,0,300,0);


        cqr[0].Scan(m[0], 0, 10);
        cqr[0].Draw(m[0]);


        cqr[1].Scan(m[1], 1, 10);
        cqr[1].Draw(m[1]);
        
        std::vector<CodeQRA> codeW;
        
        cv::Vec3d xyzth(20,20,30);

        for(size_t i=0;i<cqr[0].codes.size();i++)
            CameraQR::EnqueueCodeQR3D(codeW, cqr[0].codes[i], xyzth);

        for(size_t i=0;i<cqr[1].codes.size();i++)
            CameraQR::EnqueueCodeQR3D(codeW, cqr[1].codes[i], xyzth);
        
        std::cout<<"total "<<codeW.size()<<" codes found\n"<<std::flush;
        
        cv::namedWindow("cam0",CV_WINDOW_NORMAL);
        cv::imshow("cam0", m[0]);
        cv::imwrite("res0.png",m[0]);

        cv::namedWindow("cam1",CV_WINDOW_NORMAL);
        cv::imshow("cam1", m[1]);
        cv::imwrite("res1.png",m[1]);
        cv::waitKey(1000);
    }





};


#endif // SCANNERQR_HPP
